#include <Servo.h>

Servo servo1; // 创建第一个舵机对象
Servo servo2; // 创建第二个舵机对象
Servo servo3; // 创建第三个舵机对象
Servo servo4; // 创建第四个舵机对象

void setup() {
  servo1.attach(9); // 将 servo1 连接到引脚 9
  servo2.attach(10); // 将 servo2 连接到引脚 10
  servo3.attach(11); // 将 servo3 连接到引脚 11
  servo4.attach(12); // 将 servo4 连接到引脚 12
  Serial.begin(9600); // 初始化串口通信，波特率为 9600
  while (!Serial) {
    ; // 等待串口连接
  }
  Serial.println("Enter servo angles in the format: s1:angle1 s2:angle2 s3:angle3 s4:angle4");
}

void loop() {
  while (Serial.available() == 0) {
    // 等待用户输入
    delay(100);
  }
  String input = Serial.readStringUntil('\n'); // 读取串口输入直到换行符
  input.trim(); // 去除前后的空格
  if (input.length() > 0) {
    int s1_angle =1, s2_angle = 1, s3_angle = 1, s4_angle = 1; // 默认角度
    // 解析输入
    int index1 = input.indexOf("s1:");
    int index2 = input.indexOf("s2:");
    int index3 = input.indexOf("s3:");
    int index4 = input.indexOf("s4:");
    if (index1!= -1) {
      s1_angle = input.substring(index1 + 3).toInt();
    }
    if (index2!= -1) {
      s2_angle = input.substring(index2 + 3).toInt();
    }
    if (index3!= -1) {
      s3_angle = input.substring(index3 + 3).toInt();
    }
    if (index4!= -1) {
      s4_angle = input.substring(index4 + 3).toInt();
    }
    // 对于引脚 12 的特别处理
    if (s4_angle < 0) {
      s4_angle = abs(s4_angle);
      // 这里可以添加你对引脚 12 负角度的特殊处理逻辑，例如反转舵机方向等
      // 例如，将负角度转换为对应的正角度，你可以根据实际需求修改逻辑
      s4_angle = 180 - s4_angle; 
    }
    // 限制角度范围在 0 到 180 度之间
    s1_angle = constrain(s1_angle, 0, 180);
    s2_angle = constrain(s2_angle, 0, 180);
    s3_angle = constrain(s3_angle, 0, 180);
    s4_angle = constrain(s4_angle, 0, 180);
    // 控制舵机
    servo1.write(s1_angle);
    servo2.write(s2_angle);
    servo3.write(s3_angle);
    servo4.write(s4_angle);
    Serial.print("Servo 1: ");
    Serial.print(s1_angle);
    Serial.print(" Servo 2: ");
    Serial.print(s2_angle);
    Serial.print(" Servo 3: ");
    Serial.print(s3_angle);
    Serial.print(" Servo 4: ");
    Serial.println(s4_angle);
  }
  // 循环控制舵机运动，例如，让舵机在一定角度范围内循环运动
  for (int i = 0; i < 180; i += 10) {
    servo1.write(i);
    servo2.write(180 - i);
    servo3.write(i);
    servo4.write(180 - i);
    delay(100);
  }
  for (int i = 180; i > 0; i -= 10) {
    servo1.write(i);
    servo2.write(180 - i);
    servo3.write(i);
    servo4.write(180 - i);
    delay(100);
  }
}